#ifndef MY_PCL_TUTORIAL_UKF_H
#define MY_PCL_TUTORIAL_UKF_H

#include "Eigen/Dense"
#include <vector>
#include <string>
#include <fstream>
#include <numeric>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <opencv2/opencv.hpp>
#include <ros/console.h>

using namespace std;
using namespace Eigen;

template<class T>
void _printVecForDebug(string f, vector<T> vec)
{
	// make a string like n x %f
	int n = vec.size();
	if(n == 0) return;

	string data = "";
	for(int i=0; i<n; i++) 	data += (to_string(vec[i]) + ", ");
	data = data.substr(0, data.size()-2);
	ROS_DEBUG_NAMED(f, "(%s)", data.c_str());

}

struct TrackingState
{
	int ISSTATIC = -1;
	int LOST = 0;
	int STABLE = 4;
	int DRIFT = 10;
};

struct UkfParams{

	UkfParams(){}

	void read(const cv::FileNode& node)
	{
		node["std_a"] >> std_a;
		node["std_yawdd"] >> std_yawdd;
		node["std_lidar"] >> std_lidar;

		node["p123"] >> p123;

		node["initSkip"] >> initSkip;
		node["initVThresh"] >> initVThresh;
		node["initYawThresh"] >> initYawThresh;
		node["vStaticThresh"] >> vStaticThresh;
		node["yawStaticThresh"] >> yawStaticThresh;

		node["predDeltaTime"] >> predDeltaTime;
	}


    vector<double> std_a;
    vector<double> std_yawdd;
    vector<double> std_lidar;
        
    vector<double> p123;
	
	int initSkip;
	double initVThresh;
    double initYawThresh;
	double vStaticThresh;
	double yawStaticThresh;


    double gammaG;
    double pD;
    double pG;

	double predDeltaTime;

};

static void read(const cv::FileNode& node, UkfParams& x, const UkfParams& default_value = UkfParams()){
    if(node.empty())
        x = default_value;
    else
        x.read(node);
}


class UKF {
public:

    ///* initially set to false, set to true in first call of ProcessMeasurement
    bool is_initialized_;

//    ///* state vector: [pos1 pos2 vel_abs yaw_angle yaw_rate] in SI units and rad
    MatrixXd x_merge_;

    ///* state vector: [pos1 pos2 vel_abs yaw_angle yaw_rate] in SI units and rad
    MatrixXd x_cv_;

    ///* state vector: [pos1 pos2 vel_abs yaw_angle yaw_rate] in SI units and rad
    MatrixXd x_ctrv_;

    ///* state vector: [pos1 pos2 vel_abs yaw_angle yaw_rate] in SI units and rad
    MatrixXd x_rm_;

//    ///* state covariance matrix
    MatrixXd P_merge_;

    ///* state covariance matrix
    MatrixXd P_cv_;

    ///* state covariance matrix
    MatrixXd P_ctrv_;

    ///* state covariance matrix
    MatrixXd P_rm_;

    MatrixXd Q_;

//    ///* predicted sigma points matrix
//    MatrixXd Xsig_pred_;

    ///* predicted sigma points matrix
    MatrixXd Xsig_pred_cv_;

    ///* predicted sigma points matrix
    MatrixXd Xsig_pred_ctrv_;

    ///* predicted sigma points matrix
    MatrixXd Xsig_pred_rm_;

    ///* time when the state is true, in us
    long long time_us_;

    ///* Process noise standard deviation longitudinal acceleration in m/s^2
    double std_a_cv_;
    double std_a_ctrv_;
    double std_a_rm_;
    ///* Process noise standard deviation yaw acceleration in rad/s^2
//    double std_yawdd_;
    // CTRV
    double std_ctrv_yawdd_;
    // CV
    double std_cv_yawdd_;

    double std_rm_yawdd_;

    ///* Laser measurement noise standard deviation position1 in m
    double std_laspx_;

    ///* Laser measurement noise standard deviation position2 in m
    double std_laspy_;

    ///* Weights of sigma points
    VectorXd weights_;

    ///* State dimension
    int n_x_;

    ///* Augmented state dimension
    int n_aug_;

    ///* Sigma point spreading parameter
    double lambda_;

    ///* Augmented sigma point spreading parameter
    double lambda_aug_;

    ///* the current NIS for laser
    double NIS_laser_;

    int count_;
    int count_empty_;

    double modeMatchProbCV2CV_;
    double modeMatchProbCTRV2CV_;
    double modeMatchProbRM2CV_ ;

    double modeMatchProbCV2CTRV_;
    double modeMatchProbCTRV2CTRV_ ;
    double modeMatchProbRM2CTRV_ ;

    double modeMatchProbCV2RM_ ;
    double modeMatchProbCTRV2RM_ ;
    double modeMatchProbRM2RM_ ;

    double modeMatchProbCV_;

    double modeMatchProbCTRV_;

    double modeMatchProbRM_;

    double modeProbCV_;
    double modeProbCTRV_;
    double modeProbRM_;

    std::vector<double> ini_u_;

    std::vector<double> p1_;

    std::vector<double> p2_;

    std::vector<double> p3_;
	
	// r means radar, l means lidar
    VectorXd zPredCVr_;
    VectorXd zPredCTRVr_;
    VectorXd zPredRMr_;
	MatrixXd rS_cv_;
	MatrixXd rS_ctrv_;
	MatrixXd rS_rm_;

    VectorXd zPredCVl_;
    VectorXd zPredCTRVl_;
    VectorXd zPredRMl_;
    MatrixXd lS_cv_;
    MatrixXd lS_ctrv_;
    MatrixXd lS_rm_;

    MatrixXd K_cv_;
    MatrixXd K_ctrv_;
    MatrixXd K_rm_;

    // Output filestreams for radar and laser NIS
//    std::ofstream NISvals_laser_cv_;
//    std::ofstream NISvals_laser_ctrv_;
//    std::ofstream NISvals_laser_rm_;

    double gammaG_;
    double pD_;
    double pG_;
	
	// some const parameter
	int init_skip_;	
	double init_v_thresh_ ;
	double init_yaw_thresh_ ;
	double v_static_thresh_ ;
	double yaw_static_thresh_ ;

    int lifetime_;
    std::vector<double> velo_history_;
    bool isStatic_;

	// object state
	int state_;
	static TrackingState trackingState_;
	// last time
	double last_time_;

	// for init
	vector<VectorXd> init_cp_;
	vector<double> init_v_;
	vector<double> init_time_;
	vector<double> init_yaw_;
	vector<pcl::PointCloud<pcl::PointXYZ>> init_boxes_;
	vector<Vector3d> init_box_xyz_;
	double init_z_cp_;

    // bounding box params
    Vector3d Bbox_XYZ_; 
	
	// for trajectory
	double pred_dt_;
	vector<vector<double>> trajState_;
	
	std::vector<double> bb_yaw_history_;
    std::vector<double> bb_vel_history_;
    std::vector<double> bb_area_history_;


    double x_merge_yaw_;
	
	Vector3d box_color_;
	int box_id_;
    
	int updateBBLastTime[2];
	
	// for debug
	int life_time_;
	
	static const double box_limit_xyz_[3];
	/**
     * Constructor
     */
    UKF();
    UKF(const UkfParams& params);
    /**
     * Destructor
     */
    virtual ~UKF();

	Vector3d getMedianXYZ(const vector<Vector3d>& vec);

    void UpdateYawWithHighProb();

	void initialize(const pcl::PointCloud<pcl::PointXYZ>& box, const VectorXd& z, double timestamp);

	void initializeForEgo(const Vector3d& z, double timestamp);

    double CalculateGauss(VectorXd z, int sensorInd, int modelInd);

    void UpdateModeProb(std::vector<double> lambdaVec);

    void MergeEstimationAndCovariance();

    void MixingProbability();

    void Interaction();

    void MeasurementValidation(VectorXd z, std::vector<VectorXd>& meas);

	void singleUpdate(VectorXd z, int modelInd);

    Vector3d getXYZLength(const pcl::PointCloud<pcl::PointXYZ>& box, const VectorXd& v, int debug=0);

    /**
     * ProcessMeasurement
     * @param meas_package The latest measurement data of either radar or laser
     */
    void ProcessIMMUKF(double timestamp, bool flag);

    void PostProcessIMMUKF(std::vector<double> lambdaVec);


    void Ctrv(double p_x, double p_y, double v, double yaw, double yawd, double nu_a, double nu_yawdd, double delta_t, std::vector<double>&state);

    void Cv(double p_x, double p_y, double v, double yaw, double yawd, double nu_a, double nu_yawdd, double delta_t, std::vector<double>&state);

    void randomMotion(double p_x, double p_y, double v, double yaw, double yawd, double nu_a, double nu_yawdd, double delta_t, std::vector<double>&state);

    /**
     * Prediction Predicts sigma points, the state, and the state covariance
     * matrix
     * @param delta_t Time between k and k+1 in s
     */
    void Prediction(double delta_t, int modelInd);

    /**
     * Updates the state and the state covariance matrix using a laser measurement
     * @param meas_package The measurement at k+1
     */
    void UpdateLidar(int modelInd);

    /**
     * Updates the state and the state covariance matrix using a radar measurement
     * @param meas_package The measurement at k+1
     */
    // void UpdateRadar(MeasurementPackage meas_package, int modelInd);

	void predictTraj(double predTime, const vector<vector<double>>& egoTraj);
    void predictTraj(double predTime, const Vector3d& z);
	
};

#endif /* UKF_H */
